/*
 * @Description:
 * @Version:
 * @Author: Troubadour
 * @Date: 2022-08-31 21:57:58
 * @LastEditors: Troubadour
 * @LastEditTime: 2022-09-25 18:52:43
 */

#include "RobotServoFw.hpp"
#include "RobotArray.hpp"


/**
 * @function: RobotServoFw
 * @brief 默认构造函数
 * @param {*}
 * @return {*}
 */
RobotServoFw::RobotServoFw()
{
    HardSerialX = &Serial;
#if !(defined(ESP8266) || defined(ESP32))
    SoftSerialX = NULL;
#endif
}


/**
 * @function: RobotServoFw
 * @brief 带指定串口ID的构造函数
 * @param {HardwareSerial} &Handle
 * @return {*}
 */
RobotServoFw::RobotServoFw(HardwareSerial &Handle)
{
    HardSerialX = &Handle;
#if !(defined(ESP8266) || defined(ESP32))
    SoftSerialX = NULL;
#endif
}


/**
 * @function: RobotServoFw
 * @brief 带指定串口ID的构造函数（虚拟串口构造函数）
 * @param {SoftwareSerial} &Handle
 * @return {*}
 */
#if !(defined(ESP8266) || defined(ESP32))
RobotServoFw::RobotServoFw(SoftwareSerial &Handle)
{
    HardSerialX = NULL;
    SoftSerialX = &Handle;
}
#endif


/**
 * @function: ~RobotServoFw
 * @brief 析构函数
 * @param {*}
 * @return {*}
 */
RobotServoFw::~RobotServoFw()
{
}


/**
 * @function: RobotCheckSum
 * @brief 计算舵机协议校验和
 * @param {uint8_t} *buf：协议数据
 * @return {uint8_t} 返回校验结果
 */
uint8_t RobotServoFw::RobotCheckSum(uint8_t (&buf)[CMD_Size])
{
    uint8_t i;
    uint16_t temp = 0;
    for (i = 2; i < buf[3] + 2; i++) {
    temp += buf[i];
    }
    temp = ~temp;
    i = (uint8_t)temp;
    return i;
}


/**
 * @function: RobotSerialServoMove
 * @brief 控制单个舵机转动
 * @param {uint8_t} id：舵机ID
 * @param {int16_t} position：舵机位置，范围（0~1000）
 * @param {uint16_t} time
 * @return {*}
 */
void RobotServoFw::RobotSerialServoMove(uint8_t id, int16_t position, uint16_t time)
{
    uint8_t buf[20];
    if(position < 0)
        position = 0;
    if(position > 1000)
        position = 1000;
    buf[0] = buf[1] = ROBOT_SERVO_FRAME_HEADER;
    buf[2] = id;
    buf[3] = 7;
    buf[4] = ROBOT_SERVO_MOVE_TIME_WRITE;
    buf[5] = GET_LOW_BYTE(position);
    buf[6] = GET_HIGH_BYTE(position);
    buf[7] = GET_LOW_BYTE(time);
    buf[8] = GET_HIGH_BYTE(time);
    buf[9] = RobotCheckSum(buf);

    if(HardSerialX != NULL)
        HardSerialX->write(buf, 10);
#if !(defined(ESP8266) || defined(ESP32))
    else if (SoftSerialX != NULL)
        SoftSerialX->write(buf, 10);
#endif
}









